/*
 ** Licensed under the Apache License, Version 2.0 (the "License");
 ** you may not use this file except in compliance with the License.
 ** You may obtain a copy of the License at
 **
 **     http://www.apache.org/licenses/LICENSE-2.0
 **
 ** Unless required by applicable law or agreed to in writing, software
 ** distributed under the License is distributed on an "AS IS" BASIS,
 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 ** See the License for the specific language governing permissions and
 ** limitations under the License.
 */
package org._649mset;

import edu.wpi.first.wpilibj.*;

public class ArcadeDT {
    // Test conflict resolution. Edited on Mac and web site.
    final int VICTOR_COUNT = 4;
    double[] pseudoDT;
    Victor[] DT;

    public ArcadeDT() {
        pseudoDT = new double[VICTOR_COUNT];
        DT = new Victor[VICTOR_COUNT];
        for(int i = 0; i < VICTOR_COUNT; i++) {
            pseudoDT[i] = 0;
            DT[i] = new Victor(i+1);
        }
    }

    public ArcadeDT(int[] pwm) {
        pseudoDT = new double[VICTOR_COUNT];
        DT = new Victor[VICTOR_COUNT];
        for(int i = 0; i < VICTOR_COUNT; i++) {
            pseudoDT[i] = 0;
            DT[i] = new Victor(pwm[i]);
        }
    }

    public double[] getPseudoDT() {
        return pseudoDT;
    }

    public Victor[] getDT() {
        return DT;
    }

    public void drive(double spd, double rot) {
        pseudoDT[0] = rot - spd;
        pseudoDT[1] = spd + rot;
        pseudoDT[2] = spd + rot;
        pseudoDT[3] = rot - spd;
        double max = pseudoDT[0];
        for(int i = 1;i < VICTOR_COUNT; i++) {
            if(max < Math.abs(pseudoDT[i])) max = pseudoDT[i];
        }
        if(max > 1) {
            for(int i = 0; i < VICTOR_COUNT; i++) {
                pseudoDT[i] /= max;
            }
        }
        for(int i = 0; i < VICTOR_COUNT; i++) {
            DT[i].set(pseudoDT[i]);
        }
    }
}